﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using OpenCvSharp;
namespace PlantPick
{

    class WorldPointTrans
    {
        static double[,] rvec1 = new double[3, 1] { { 2.218904371501898 }, { 2.155918223387875 }, { 0.01535863376060535 } };
        static double[,] tvec1 = new double[3, 1] { { -6.348215724915176 }, { -433.741937426114 },  { 496.0668976226461 } };
        static double[,] cameraMatrix1 = new double[3, 3] { { 477.9457219776931, 0, 335.6672967270359 }, { 0, 479.9887289438592, 168.2704914622266 }, { 0, 0, 1 } };
        public static Mat matRvec = new Mat(3, 1, MatType.CV_64F, rvec1);
        public static Mat mattvec = new Mat(3, 1, MatType.CV_64F, tvec1);
        public static Mat matcameraMatrix = new Mat(3, 3, MatType.CV_64F, cameraMatrix1);

        public Point3d getWorldPoints(Point2f inPoints, Mat rvec , Mat tvec, Mat cameraMatrix)
        {
            
            //Console.WriteLine(Cv2.Format(rvec));
            //Console.WriteLine(Cv2.Format(tvec));
            //Console.WriteLine(Cv2.Format(cameraMatrix));
            //initialize parameter
            Mat rotationMatrix = new Mat(3, 3, MatType.CV_64F);//3*3
            Cv2.Rodrigues(rvec, rotationMatrix); /* 将旋转向量转换为相对应的旋转矩阵 */
            double zConst = 0;//实际坐标系的距离，若工作平面与相机距离固定可设置为0
            double s;
            //Console.WriteLine(Cv2.Format(rotationMatrix));
            //获取图像坐标
            Mat imagePoint = new Mat(3, 1, MatType.CV_64F, new double[3,1]{ { inPoints.X}, { inPoints.Y }, { 1 } });
            // cv::Mat::ones(3, 1, cv::DataType<double>::type); //u,v,1
            // imagePoint.at<double>(0, 0) = inPoints.x;
            // imagePoint.at<double>(1, 0) = inPoints.y;

            //计算比例参数S
            Mat tempMat, tempMat2;
            tempMat = rotationMatrix.Inv() * cameraMatrix.Inv() * imagePoint;
            tempMat2 = rotationMatrix.Inv() * tvec;
            s = zConst + tempMat2.At<double>(2, 0);
            s /= tempMat.At<double>(2, 0);
            //Console.WriteLine(Cv2.Format(tempMat));
            //Console.WriteLine(Cv2.Format(tempMat2));
            //Console.WriteLine(Cv2.Format(s));
            //计算世界坐标
            Mat wcPoint = rotationMatrix.Inv() * (s * cameraMatrix.Inv() * imagePoint - tvec);
            //Console.WriteLine(Cv2.Format(wcPoint));
            Point3d worldPoint = new Point3d(wcPoint.At<double>(0, 0), wcPoint.At<double>(1, 0), wcPoint.At<double>(2, 0));
            return worldPoint;
        }
    }
}
